function plot_fastslam_laser(true_pose, particles, true_traj, est_traj, obstacles, time)
% FastSLAM激光雷达可视化

    clf;
    
    % 左图：地图+粒子
    subplot(1, 2, 1); hold on; grid on; axis equal;
    
    % 障碍物
    for i = 1:size(obstacles, 1)
        obs = obstacles(i, :);
        if i <= 4
            rectangle('Position', [obs(1), obs(2), obs(3)-obs(1), obs(4)-obs(2)], ...
                     'FaceColor', [0.3, 0.3, 0.3], 'EdgeColor', 'k', 'LineWidth', 1.5);
        else
            rectangle('Position', [obs(1), obs(2), obs(3)-obs(1), obs(4)-obs(2)], ...
                     'FaceColor', [0.7, 0.7, 0.7], 'EdgeColor', 'k');
        end
    end
    
    % 真实轨迹
    plot(true_traj(1, :), true_traj(2, :), 'g-', 'LineWidth', 2);
    
    % 估计轨迹
    plot(est_traj(1, :), est_traj(2, :), 'b--', 'LineWidth', 1.5);
    
    % 粒子位姿
    N = length(particles);
    for i = 1:N
        pose = particles(i).pose;
        w = particles(i).weight;
        markersize = max(2, w * N * 30);
        plot(pose(1), pose(2), 'b.', 'MarkerSize', markersize);
    end
    
    % 真实机器人
    draw_robot(true_pose(1), true_pose(2), true_pose(3), 0.8, 'g');
    
    % 最佳粒子的路标
    [~, best_idx] = max([particles.weight]);
    best_particle = particles(best_idx);
    if ~isempty(best_particle.landmarks)
        n_lm = length(best_particle.landmarks);
        est_lm = zeros(n_lm, 2);
        for i = 1:n_lm
            est_lm(i, :) = best_particle.landmarks(i).mean';
        end
        plot(est_lm(:, 1), est_lm(:, 2), 'r*', 'MarkerSize', 8, 'LineWidth', 2);
    end
    
    xlabel('X(m)'); ylabel('Y(m)');
    title(sprintf('FastSLAM+激光 (t=%.1fs, 粒子:%d)', time, N));
    xlim([0, 50]); ylim([0, 50]);
    legend('真实轨迹', '估计轨迹', 'Location', 'best');
    
    % 右图：全局视图
    subplot(1, 2, 2); hold on; grid on; axis equal;
    
    for i = 1:size(obstacles, 1)
        obs = obstacles(i, :);
        rectangle('Position', [obs(1), obs(2), obs(3)-obs(1), obs(4)-obs(2)], ...
                 'FaceColor', [0.8, 0.8, 0.8], 'EdgeColor', 'k');
    end
    
    plot(true_traj(1, :), true_traj(2, :), 'g-', 'LineWidth', 1.5);
    plot(est_traj(1, :), est_traj(2, :), 'b--', 'LineWidth', 1.5);
    draw_robot(true_pose(1), true_pose(2), true_pose(3), 1.0, 'g');
    
    % 粒子云
    for i = 1:N
        pose = particles(i).pose;
        plot(pose(1), pose(2), 'b.', 'MarkerSize', 4);
    end
    
    xlabel('X(m)'); ylabel('Y(m)');
    title('全局视图');
    xlim([0, 50]); ylim([0, 50]);
    legend('真实', '估计', 'Location', 'best');
end

function draw_robot(x, y, theta, size, color)
    robot_points = size * [0.5, 0; -0.5, 0.3; -0.5, -0.3; 0.5, 0];
    R = [cos(theta), -sin(theta); sin(theta), cos(theta)];
    robot_points = (R * robot_points')';
    robot_points(:, 1) = robot_points(:, 1) + x;
    robot_points(:, 2) = robot_points(:, 2) + y;
    plot(robot_points(:, 1), robot_points(:, 2), color, 'LineWidth', 2);
end

